#ifndef AER_SORTINDEX_HPP_
#define AER_SORTINDEX_HPP_

#include "aerDefs.hpp"

#include <algorithm>
#include <vector>


namespace aer {
  
/// =========================================
/// Creates a buffer of sorted indices along 
/// an arbitrary axis for a given buffer of 
/// coordinates
/// =========================================
struct SortIndex
{
  private:
    struct SortL
    {
      const float *attrib;      
      SortL(const float *attrib) : attrib(attrib) {}
      
      bool operator() (unsigned int i1, unsigned int i2) { 
        return attrib[i1] < attrib[i2];
      }
    };
    
  
  private:
    std::vector<unsigned int> m_indices;
    std::vector<float> m_attribs;             // Values used to sorted datas
    
  
  public:
    SortIndex()
    {
      m_indices.clear();
      m_attribs.clear();
    }
        
    /// TODO : accept buffer of const Position3D to sort
    
    
    /// Generic sorting of an attrib j of any N-vector buffer (j < N)
    template<unsigned int nDim>
    void sortDim(const size_t nElems, const unsigned int dim, const float *data)
    {
      AER_ASSERT( (dim >= 0u) && (dim < nDim) );
      
      resetIndices( nElems );
      resetAttribs<nDim>( nElems, dim, data);
      
      sort();
    }
    
    
    void sortX( const size_t nElems, const float *data, unsigned int *dst=NULL) 
    { 
      sortDim<3>( nElems, 0, data); 
      
      if (dst != NULL) {
        copy(dst);
      }
    }
    
    void sortY( const size_t nElems, const float *data, unsigned int *dst=NULL) 
    { 
      sortDim<3>( nElems, 1, data); 
      
      if (dst != NULL) {
        copy(dst);
      }
    }
    
    void sortZ( const size_t nElems, const float *data, unsigned int *dst=NULL) 
    { 
      sortDim<3>( nElems, 2, data); 
      
      if (dst != NULL) {
        copy(dst);
      }
    }
        
    void sort3f( const size_t nElems, const float *data, const float axis[3], 
                 unsigned int *dst=NULL)
    {
      resetIndices( nElems );
      
      // normalize axis (not really needed)
      float invSqrt = (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]);

      if (invSqrt <= 0.0f) return;      
      invSqrt = sqrtf( 1.0f / invSqrt );
      
      float sortAxis[3];
      sortAxis[0] = axis[0] * invSqrt;
      sortAxis[1] = axis[1] * invSqrt;
      sortAxis[2] = axis[2] * invSqrt;      
      
      // reset attribs (project datas on axis)      
      m_attribs.resize( nElems );
      
      for (unsigned int i=0u; i<nElems; ++i) 
      {
        const float *v = &data[3*i];        
        m_attribs[i] = v[0]*sortAxis[0] + v[1]*sortAxis[1] + v[2]*sortAxis[2];
      }
      
      sort();
      
      if (dst != NULL) {
        copy(dst);
      }
    }
  
    void copy(unsigned int *dst)
    {
      std::copy( m_indices.begin(), m_indices.end(), dst);
    }
    
    unsigned int getSize() { return m_indices.size(); }
    unsigned int getIndex(const unsigned int idx) const { return m_indices[idx]; }
    const unsigned int* getIndices() const { return m_indices.data(); }
    
    
  private:
    void sort()
    {
      std::sort( m_indices.begin(), m_indices.end(), SortL(m_attribs.data()));
      m_attribs.clear();//
    }
    
    void resetIndices(const U32 nElems)
    {
      m_indices.resize( nElems );
      for (size_t i=0u; i<nElems; ++i) {
        m_indices[i] = i;
      }
    }
    
    template<unsigned int nDim>
    void resetAttribs(const U32 nElems, const U8 dim, const float *data)
    {
      m_attribs.resize( nElems );
      for (unsigned int i=0u; i<nElems; ++i) {
        m_attribs[i] = data[i*nDim + dim];
      }
    }
};

} // aer

#endif //AER_SORTINDEX_HPP_
